Quadrotor Quickstart

This is a walkthrough example on how to use this library. The whole point is to make it easily extensible, and to allow users to easily swap in different controllers and robotic systems.

We will simulate a quadrotor executing a 3D Lissajous trajectory. In the end, we will produce the following animation:

Define the quadrotor

using ComponentArrays, Plots, LinearAlgebra, Parameters
using RoboticSystems
RS = RoboticSystems

gr()   # uncomment for fast (but non-interactive) 3d plots
# plotly() # uncomment for interactive 3d plots. Does not support animations.

## Define parameters of the quad
# start with default parameters, and modify some values
quad_p = ComponentArray(
  RS.quadrotor_parameters;
  mass=1.5,
  k_drag_f =  0.02
) |> RS.initialize_quad_params

## Define the quad's initial condition
quad_ic = ComponentArray(
    x = zeros(3),
    v = zeros(3),
    R = 1.0(I(3)) |> collect,
    Ω = zeros(3),
    ω = 2*RS.hover_ω(quad_p)
);

##Plot the quadrotor
plot()
RS.plot_quad!(quad_ic, quad_p)
RS.plot_triad!(quad_ic)
RS.plot_iso3d!()

Notes:

• the initial state of the quadrotor is expressed using a ComponentArray. This is an incredible library, and is the keytool to allow us to compose various equations together.
• notice the |> when we constructed quad_p: we passed quad_p through initialize_quad_params. This is necessary to initialize (and invert) some commonly used matrices, to avoid runtime cost
• RoboticSystems provides some convenient plotting tools.
  • RS.plot_quad! plots a quadrotor
  • RS.plot_triad plots the XYZ axes of a rotation matrix as red (x) green (y) and blue (z) lines. By default, we only use ENU frames, and quad_ic.R is a rotation matrix from the quadrotor-body fixed frame to to inertial frame.
  • RS.plot_iso3d! adjusts x,y,zlims to make the plot look like aspect_ratio=1

Define the closed-loop function

Next, we need to define the closed-loop dynamics, by using a feedback controller. The controller used in this example is a geometric controller that uses differential flatness to track a path.

## Define a trajectory
# a Lissajous trajectory
trajectory_params = ComponentArray(
    A = [1, 1, 0.2],
    ω = [5/8,4/8,6/8],
    ϕ = [π/2, 0, 0],
    off = [0,0,1.]
)

## Define the controller

controller_params = ComponentArray(
    kx = 1.0,
    kv = 2.0,
    kR = 0.35,
    kΩ = 0.15,
    m = 1.5,
    J = quad_p.J, # this is cheating, but is convenient,
    invG = quad_p.invG, # this is also cheating...
    g = 9.81,
    trajectory_params = trajectory_params
)

function controller(state, controller_params, t)

    @unpack trajectory_params = controller_params

    ## define the desired position, velocity, acceleration, jerk, snap at the current time
    xd = RS.lissajous(t, trajectory_params)
    vd = RS.lissajous(t, trajectory_params, 1)
    ad = RS.lissajous(t, trajectory_params, 2)
    jd = RS.lissajous(t, trajectory_params, 3)
    sd = RS.lissajous(t, trajectory_params, 4)

    ## define the desired yaw, yaw rate, and yaw acceleration at current time
    ψ = 3.0*t/8
    ψd = 3.0/8
    ψdd = 0.0

    ## convert into target quadrotor state,  using differential flatness
    target = RS.flat_state_to_quad_state(xd, vd, ad, jd, sd, ψ, ψd, ψdd)

    ## determine required thrust and moments, using geometric controller
    fM = RS.geometric_controller(state, target..., controller_params)
    # returns a vector [f, M[1], M[2], M[3]]

    ## determine rotation speed of each motor
    control = RS.fM_to_ω(fM, controller_params)
    # returns the desired ω of each motor

end


## define the closed loop dynamics
function cl_geometric_quad!(D, state, params, t)

    @unpack quad_params, controller_params = params

    u = controller(state, controller_params, t)

    # define the external wind speed (constant, for simplicity)
    w_ext = [1.0, 0, 0.]

    # determine the rate of change of state based on control input and other forces/torques
    RS.quadrotor!(D, state, quad_params, u, t; wind_ext=w_ext)

end

cl_geometric_quad! (generic function with 1 method)

You can imagine more complicated wind speeds can be modelled. Additional external forces or torques can be specified in RS.quadrotor!.

Perform the Simulation

We can use the excellent library DifferentialEquations.jl to achieve high performance and extensible ODE solving. RoboticSystems.jl doesnt require you to use DiffEq.jl, but it is written in a way that makes it easy to use DiffEq.jl.

using DifferentialEquations

tspan = (0, 100.0)

# combine all the parameters
params = ComponentArray(
  quad_params = quad_p,
  controller_params = controller_params
)

# define the ODE problem
prob = ODEProblem(cl_geometric_quad!, quad_ic, tspan, params)

## Solve the problem
sol = solve(prob, Tsit5())

retcode: Success
Interpolation: specialized 4th order "free" interpolation
t: 1407-element Vector{Float64}:
   0.0
   8.92818794202627e-5
   0.0009821006736228895
   0.003036647653795071
   0.005987461553222742
   0.009981227065692003
   0.015346536913553137
   0.022374801108247425
   0.031527651450210234
   0.04309666110570046
   ⋮
  99.44648738635581
  99.51830055207641
  99.59025435359021
  99.6623959991283
  99.7346720414413
  99.80701021714177
  99.87934537670041
  99.95162778939407
 100.0
u: 1407-element Vector{ComponentArrays.ComponentVector{Float64, Vector{Float64}, Tuple{ComponentArrays.Axis{(x = 1:3, v = 4:6, R = ViewAxis(7:15, ShapedAxis((3, 3), NamedTuple())), Ω = 16:18, ω = 19:22)}}}}:
 ComponentVector{Float64}(x = [0.0, 0.0, 0.0], v = [0.0, 0.0, 0.0], R = [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], Ω = [0.0, 0.0, 0.0], ω = [2775.641994507828, 2775.641994507828, 2775.641994507828, 2775.641994507828])
 ComponentVector{Float64}(x = [5.314168202498758e-11, 6.618016817477421e-19, 1.1707462655765928e-7], v = [1.1904250305974494e-6, 3.703237619185394e-14, 0.002620097552199617], R = [0.9999999999999999 -1.345174389946138e-8 2.840740619749971e-11; 1.3451743899460178e-8 0.9999999999999999 4.2408733280914876e-11; -2.8407406767989885e-11 -4.240873289877319e-11 1.0], Ω = [-1.424028910936937e-6, 9.538749015553652e-7, 0.0003020917746086717], ω = [2769.6637708017015, 2769.6333254750016, 2769.7274239349667, 2769.881392660908])
 ComponentVector{Float64}(x = [6.430588420047734e-9, 1.0221621824712808e-13, 1.3901634180329061e-5], v = [1.3096589508115594e-5, 5.15749057071848e-10, 0.02801855339054633], R = [0.9999999999985416 -1.7073990086031888e-6 3.704027727741963e-8; 1.7073990065342194e-6 0.9999999999985408 5.531083130000745e-8; -3.704037176998225e-8 -5.531076802029433e-8 0.9999999999999978], Ω = [-0.0001676988904998352, 0.00011229324454817001, 0.003563766406289183], ω = [2711.2733123698263, 2710.9445097048124, 2711.9514725335343, 2713.6112920266605])
 ComponentVector{Float64}(x = [6.152730387193882e-8, 2.630460103102323e-11, 0.00012738322688478812], v = [4.056191893946913e-5, 4.213411197356991e-8, 0.0813049241058527], R = [0.9999999998389366 -1.7916704261929145e-5 1.0444085249215207e-6; 1.7916702627036678e-5 0.9999999998382624 1.5605604241880948e-6; -1.0444364985026708e-6 -1.5605417023542635e-6 0.9999999999982315], Ω = [-0.0015063844722949576, 0.0010078555253564023, 0.012524111091468358], ω = [2586.0719776062792, 2585.0995722666225, 2588.0207539547255, 2592.9085149313996])
 ComponentVector{Float64}(x = [2.399414736922327e-7, 6.887480397157932e-10, 0.00046694881020027505], v = [8.050466332231332e-5, 5.456419223776936e-7, 0.14686226287253173], R = [0.9999999969932308 -7.71835515561095e-5 7.483222260377458e-6; 7.718346774671266e-5 0.9999999969585784 1.1192716623832484e-5; -7.48408625578526e-6 -1.1192138925059042e-5 0.9999999999093093], Ω = [-0.005356595983141624, 0.003579202465050727, 0.02808448991260358], ω = [2426.6385479300548, 2424.8488879886604, 2430.101337407199, 2439.0386875009076])
 ComponentVector{Float64}(x = [6.724564168636131e-7, 7.511622757822137e-9, 0.0012028747734636092], v = [0.0001365980118498079, 3.4607612545215498e-6, 0.218808579734113], R = [0.9999999714662786 -0.0002367777642008057 3.166480110128531e-5; 0.0002367762617485082 0.9999999708427946 4.7435437320745835e-5; -3.167603244544783e-5 -4.742793808313919e-5 0.9999999983733178], Ω = [-0.01320601689510684, 0.008806624339025883, 0.05226652670552954], ω = [2244.0837414661914, 2241.388472859074, 2249.1141751512314, 2262.4458457882506])
 ComponentVector{Float64}(x = [1.6205234883815043e-6, 5.2490119736888214e-8, 0.0025842689137849676], v = [0.0002182601853470752, 1.5164890005470314e-5, 0.2923280081350489], R = [0.999999808664043 -0.0006101190839572103 0.00010209566960987038; 0.0006101034249550979 0.9999998021304877 0.00015332592703933419; -0.0001021891985044737 -0.0001532636074377839 0.9999999830326288], Ω = [-0.026647677738861605, 0.017715694617609228, 0.08708437244740404], ω = [2048.3864060422675, 2044.8175370644092, 2054.8665294295038, 2072.264050028121])
 ComponentVector{Float64}(x = [3.574023870732133e-6, 2.7059413835752797e-7, 0.004893227557993308], v = [0.0003410359157332293, 5.154584921048957e-5, 0.36031531944045886], R = [0.9999990079830229 -0.0013821583114021641 0.0002714192135276432; 0.001382047172192771 0.9999989612365603 0.0004092263060416773; -0.0002719845500877032 -0.0004088507840543077 0.9999998794298747], Ω = [-0.04627207712173942, 0.030614801725061026, 0.13219760141112022], ω = [1858.3730910756187, 1854.1762110332422, 1865.9954473225844, 1885.991260170912])
 ComponentVector{Float64}(x = [7.552550933284999e-6, 1.1347106504340555e-6, 0.008474902033879373], v = [0.0005354336596651468, 0.00014750761996967763, 0.41771781501307725], R = [0.9999957663899104 -0.0028417469602004045 0.0006258492023571512; 0.0028411519780696917 0.9999955131242422 0.0009495239502824135; -0.0006285446953326485 -0.0009477418021083012 0.999999353356962], Ω = [-0.07124114655082653, 0.04677905702408864, 0.1854006326191675], ω = [1691.0942779759293, 1686.7491382548474, 1699.4808689509675, 1719.3950752547826])
 ComponentVector{Float64}(x = [1.5477505742824245e-5, 3.996228415951898e-6, 0.013579265312095327], v = [0.0008479190686686156, 0.0003671773548636568, 0.46060391230336406], R = [0.9999850409304443 -0.005321575414999373 0.0012644304532964242; 0.005319120971147042 0.9999839780526486 0.0019366551112819427; -0.0012747162167099232 -0.0019299005065488656 0.999997325303072], Ω = [-0.09781427934323014, 0.06347871117511751, 0.24094089086996107], ω = [1563.1883757887122, 1559.332602128845, 1571.9906315210626, 1588.4347234478623])
 ⋮
 ComponentVector{Float64}(x = [0.7851517451544646, -0.517385392672054, 0.8546172635563027], v = [0.39163310197634027, 0.4262530973359216, 0.10299872332281823], R = [0.9174726500989798 0.39660299321802506 -0.03082212540752428; -0.39637577236378846 0.9179902253086722 0.013423538491127473; 0.03361822412134675 -9.858544861343937e-5 0.9994347452876822], Ω = [0.016195395211837535, -0.01067879041451169, 0.37460755241732363], ω = [1394.3203328467898, 1394.3042878831836, 1394.579045963247, 1394.605793320351])
 ComponentVector{Float64}(x = [0.8124955669288082, -0.4864498430302362, 0.8622293361711004], v = [0.36977279532520463, 0.4352059091314117, 0.10872693311602043], R = [0.9277846709799094 0.3717449924448038 -0.03195725608101555; -0.3715296525454672 0.9283348750066762 0.01265208247359027; 0.034370382367126265 0.00013466033187073157 0.999409157196586], Ω = [0.015791407280119258, -0.010258653967812177, 0.37461021525322796], ω = [1394.028048850515, 1394.016485272783, 1394.2874765273407, 1394.317450782004])
 ComponentVector{Float64}(x = [0.8382913263830666, -0.4548295184323272, 0.8702573305910213], v = [0.3471226681013647, 0.4436010104652221, 0.11414620122930227], R = [0.9374427920207689 0.34656860157420016 -0.03303358027869149; -0.34636561457150583 0.9380245731784654 0.011864199141297162; 0.035098067686877614 0.00031968866369217174 0.9993838242912684], Ω = [0.015398124346364844, -0.00980118957477719, 0.374613633679979], ω = [1393.723586981723, 1393.7167365763928, 1393.9840027298558, 1394.0169075602287])
 ComponentVector{Float64}(x = [0.8624926355707049, -0.4225413638558157, 0.8786856370671561], v = [0.32371010364342956, 0.4514298640459212, 0.11924167629040458], R = [0.9464418770927354 0.32107385311694037 -0.03404930939177916; -0.3208836237059295 0.9470540474462382 0.011060258529942189; 0.035797694842195964 0.0004579742733317204 0.9993589545904165], Ω = [0.015018150892951181, -0.00930722799245207, 0.3746178772845095], ω = [1393.4087563244316, 1393.4068152236748, 1393.6704342440864, 1393.7059305773992])
 ComponentVector{Float64}(x = [0.885021244227626, -0.3896483101912223, 0.8874856518431821], v = [0.2995967742889161, 0.4586733995179064, 0.12399290562134763], R = [0.9547642733367643 0.29529661788918476 -0.035001302381420706; -0.29511947224974827 0.9554054644169835 0.01024176387314975; 0.03646479254516615 0.0005510960179637269 0.9993347887566941], Ω = [0.014654540049389125, -0.008778477231424994, 0.37462298736369576], ω = [1393.0827866455384, 1393.0859111602851, 1393.3459982615384, 1393.38370850299])
 ComponentVector{Float64}(x = [0.9058018491647036, -0.3562247748837043, 0.8966239794432744], v = [0.274854650334844, 0.46531340087062634, 0.12838041309049852], R = [0.9623929095194965 0.26928061249779955 -0.03588649871255543; -0.2691168010100979 0.9630615702727037 0.009410491187361085; 0.037094969396757986 0.0006010701499072091 0.9993115664228613], Ω = [0.01431028017766267, -0.008217117786318897, 0.3746289730121695], ω = [1392.7445786082963, 1392.7528915902028, 1393.009585296256, 1393.0491165339365])
 ComponentVector{Float64}(x = [0.9247714454651499, -0.3223446398609663, 0.9060657445767731], v = [0.2495570747110416, 0.4713354440230384, 0.13238753965127284], R = [0.9693146321194046 0.2430680684900665 -0.03670230775908731; -0.24291777401512685 0.9700090407983674 0.008568191398224961; 0.03768422285218748 0.0006103700569139124 0.9992895134180778], Ω = [0.01398810961017626, -0.0076255829467529014, 0.3746358139203767], ω = [1392.3932456957973, 1392.4068383929618, 1392.6602947614376, 1392.7012487796726])
 ComponentVector{Float64}(x = [0.941880790354409, -0.28807660055025863, 0.9157761977126387], v = [0.2237749158245847, 0.4767292556023974, 0.13600074312825658], R = [0.9755205861565445 0.21669640951359137 -0.037446674727879034; -0.21655975801535698 0.9762388669079732 0.007716476783805456; 0.0382290309038602 0.0005818613470671396 0.9992688364342658], Ω = [0.013690459629632513, -0.007006446339886582, 0.3746434623717763], ω = [1392.0282955943044, 1392.0472281293733, 1392.2976181067504, 1392.3395960537227])
 ComponentVector{Float64}(x = [0.9522824709076362, -0.2649361826317294, 0.9224122263670387], v = [0.2062633263509368, 0.4799881787188099, 0.13829092660241488], R = [0.9792746603022855 0.19895827594204113 -0.03790442564551146; -0.1988308093044703 0.9800078097505607 0.007141440095172385; 0.03856748056047362 0.0005431368125075653 0.9992558527628539], Ω = [0.013504393168915533, -0.006574853109672898, 0.3746494592297701], ω = [1391.4458752471082, 1391.4676361479108, 1391.7176801196294, 1391.7577356944648])

All of the arguments (e.g. restol, abstol) that you normally pass into ODEProblem or solve can be used. Refer to the DifferentialEquations.jl documentation for more details.

Plot/Animate the solution

plotly()

plot()
RS.plot_quad_traj!(sol, quad_p; tspan=tspan)

plot!(xlims=(-1.5, 1.5), ylims=(-1.5, 1.5), zlims=(0, 1.5))

RS.plot_iso3d!()
RS.plot_project3d!()

Next, we can use Plots.jl's animation tools to construct a simple animation.

gr()

anim = @animate for tm = range(sol.t[1], sol.t[end], 200)

    tspan=(sol.t[1], tm)

    plot()
    RS.plot_quad_traj!(sol, quad_p; tspan=tspan)

    plot!(xlims=(-1.5, 1.5), ylims=(-1.5, 1.5), zlims=(0, 1.5))

    # rotate camera
    plot!(camera = (360 * ((tm - sol.t[1]) / (sol.t[end] - sol.t[1])), 30))
    plot!(axis = ([], false), xlabel="", ylabel="", zlabel="")
    RS.plot_iso3d!()
    RS.plot_project3d!()

end;

gif(anim)

To plot any additional plot, you can use sol as a function, for example:

gr()
plot(t-> RS.roll(sol(t).R) * 180 / π, tspan..., label="sim")
plot!(xlabel="time (s)", ylabel="roll (degrees)")